/* Copyright (c) 2008, Thomas Paul Carlson <tpc4@kent.ac.uk>
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*     * Redistributions of source code must retain the above copyright
*       notice, this list of conditions and the following disclaimer.
*     * Redistributions in binary form must reproduce the above copyright
*       notice, this list of conditions and the following disclaimer in the
*       documentation and/or other materials provided with the distribution.
*     * Neither the name of the <organization> nor the
*       names of its contributors may be used to endorse or promote products
*       derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THOMAS PAUL CARLSON ``AS IS'' AND ANY
* EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL <copyright holder> BE LIABLE FOR ANY
* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

// C Wrapper for the python_nxt library

#include <stdio.h>
#include "Python.h"

/*
 * Powers the motor on at a given speed for a time until the tacho limit is
 * reached. 0 may be specified if the motor does not need to stop.
 * nxtport is the output port to activate (a - 0, b - 1, c - 2)
 */
void nxt_motor_setspeed (int speed, int limit, int nxtport)
{
    // Setup the speed, tacho limit and motor to be used
    Py_Initialize ();
    char temp[16];
    sprintf (temp, "power = %d", speed);
    PyRun_SimpleString (temp);
    sprintf (temp, "tacho = %d", limit);
    PyRun_SimpleString (temp);
    sprintf (temp, "port = %d", nxtport);
    PyRun_SimpleString (temp);
    
    // Run the python that will be doing the hard work!
    FILE* motorcode;
    motorcode = fopen ("motors.py", "r");
    PyRun_SimpleFile(motorcode, "motors.py");
    Py_Finalize ();
    fclose (motorcode);
}

/*
 *  Plays a sound on the NXT brick for a given duration
 */
void nxt_playsound (int frequency, int duration)
{
    Py_Initialize ();
    char temp[16];
    sprintf (temp, "frequency = %d", frequency);
    PyRun_SimpleString (temp);
    sprintf (temp, "duration = %d", duration);
    PyRun_SimpleString (temp);
    
    // Run the python to play the sound
    FILE* soundcode;
    soundcode = fopen ("sounds.py", "r");
    PyRun_SimpleFile (soundcode, "sounds.py");
    Py_Finalize ();
    fclose (soundcode);
}


/*
 * Get a value back from a file. Takes a filename and portnumber as parameters
 */
int nxt_sensor_generic (char * filename, int portnum)
{
    Py_Initialize ();
    PyObject* evalModule;
    PyObject* evalDict;
    PyObject* evalVal;
    long retlong;
    
    char temp[16];
    sprintf (temp, "nxtport = %d", portnum);
    PyRun_SimpleString (temp);
    
    FILE* sensorcode;
    sensorcode = fopen (filename, "r");
    PyRun_SimpleFile (sensorcode, filename);

    evalModule = PyImport_AddModule ("__main__");
    evalDict = PyModule_GetDict (evalModule);

    evalVal = PyDict_GetItemString (evalDict, "result");
    retlong = PyInt_AsLong (evalVal);

    Py_Finalize (); 
    return retlong;
}

/*
 *  Get the value of the touch sensor. 1 for pressed, 0 for not pressed
 *  Takes a single value, the port number the NXT's touch sensor is plugged into
 */
int nxt_sensor_touch (int portnum)
{
    return nxt_sensor_generic ("touch.py", portnum);
}

/*
 * Get the value of the sound sensor
 * Takes a single value, port number the NXT's sound sensor is plugged into
 */
int nxt_sensor_sound (int portnum)
{
    return nxt_sensor_generic ("sound.py", portnum);
}

/*
 * Gets the current value of the light sensor
 * Takes a single value, port number the NXT's light sensor is plugged into
 */
int nxt_sensor_light (int portnum)
{
    return nxt_sensor_generic ("light.py", portnum);
}

/*
 * Gets the current value of the ultrasound sensor
 * Takes a single value, port number the NXT's ultrasound sensor is plugged into
 */
int nxt_sensor_ultrasound (int portnum)
{
    return nxt_sensor_generic ("ultra.py", portnum);
}

/*
 * Main function. Use this to see if things are working or not!
 */
int main(int argc, const char* argv[])
{
    printf ("Testing...\n");
    printf ("Touch Sensor: %d\n", nxt_sensor_touch (1));
    printf ("Sound Sensor: %d\n", nxt_sensor_sound (2));
    printf ("Light Sensor: %d\n", nxt_sensor_light (3));
    printf ("Ultra Sensor: %d\n", nxt_sensor_ultrasound (4));
    nxt_motor_setspeed (100, 360, 0);
    nxt_motor_setspeed (75, 360, 1);
    nxt_motor_setspeed (50, 180, 2);
    nxt_playsound (587, 1000);
    return 0;
}
